﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Skill.Ode;
using Skill.Ode.Joints;
using Skill.Ode.Geometries;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Skill.Ode.Extension;
using Microsoft.Xna.Framework.Input;
using Samples.Common;

namespace Samples.Demos
{
    public class Buggy : OdeDemo
    {
        // some constants
        const float LENGTH = 0.7f;	// chassis length
        const float WIDTH = 0.5f;	// chassis width
        const float HEIGHT = 0.2f;	// chassis height
        const float RADIUS = 0.18f;	// wheel radius
        const float STARTY = 0.5f;	// starting height of chassis
        const float CMASS = 1f;		// chassis mass
        const float WMASS = 0.2f;	// wheel mass
        const int N = 10;

        const float MAXSPEED = 20;
        const float STEPSPEED = 0.2f;
        const float MAXSTEER = MathHelper.Pi / 16;

        //// dynamics and collision objects (chassis, 3 wheels, environment)
        private Ode ode;
        private DSpace space;
        private DSpace car_space;

        private MovableEntity<DBox> chassis;
        private MovableEntity<DSphere>[] wheels;
        private DHinge2[] joint = new DHinge2[3];	// joint[0] is the front wheel        
        private Entity<DPlane> ground;
        private Entity<DBox> ground_box;
        private DNearCallback callback = null;

        // things that the user controls
        private float speed = 0, steer = 0;	// user commands        

        // this is called by dSpaceCollide when two objects in space are
        // potentially colliding.

        private void NearCallback(object data, DSpace o1, DSpace o2)
        {
            int i, n;

            // only collide things with the ground
            bool g1 = (o1 == ground.Geometry) || (o1 == ground_box.Geometry);
            bool g2 = (o2 == ground.Geometry) || (o2 == ground_box.Geometry);
            if (!(g1 ^ g2))
                return;

            DContactInfoArray contacts = new DContactInfoArray(N);
            n = DGeometry.Collide(o1, o2, N, contacts);
            if (n > 0)
            {
                for (i = 0; i < n; i++)
                {
                    DContactInfo info = contacts[i];
                    info.Surface.Mode = DSurfaceParametersMode.Slip1 | DSurfaceParametersMode.Slip2 |
                   DSurfaceParametersMode.SoftERP | DSurfaceParametersMode.SoftCFM | DSurfaceParametersMode.Approx1;
                    info.Surface.Mu = float.PositiveInfinity;
                    info.Surface.Slip1 = 0.1f;
                    info.Surface.Slip2 = 0.1f;
                    info.Surface.SoftErp = 0.5f;
                    info.Surface.SoftCfm = 0.3f;
                    DContact c = new DContact(ode.World, ode.Group, info);
                    c.Attach(info.Geom.Geom1.Body, info.Geom.Geom2.Body);
                }
            }
        }


        #region Constructor
        public Buggy(DemoFramework framework)
            : base(framework)
        {
            callback = new DNearCallback(NearCallback);
            mapper.MapKeyDown(Keys.Down, delegate { speed += STEPSPEED; if (speed > MAXSPEED)speed = MAXSPEED; });
            mapper.MapKeyDown(Keys.Up, delegate { speed -= STEPSPEED; if (speed < -MAXSPEED)speed = -MAXSPEED; });
            mapper.MapKeyDown(Keys.Left, delegate { steer -= 0.02f; if (steer > MAXSTEER)steer = MAXSTEER; });
            mapper.MapKeyDown(Keys.Right, delegate { steer += 0.02f; if (steer < -MAXSTEER)steer = -MAXSTEER; });
            mapper.MapKeyDown(Keys.Space, delegate { steer = 0f; speed = 0; });
        }
        #endregion

        #region OdeDemo methods
        public override void StepDemo(Microsoft.Xna.Framework.GameTime gameTime)
        {
            mapper.Update(ref Framework.Keyboard);
            if (steer > 0)
            {
                steer -= 0.01f;
                if (steer < 0)
                    steer = 0;
            }
            else if (steer < 0)
            {
                steer += 0.01f;
                if (steer > 0)
                    steer = 0;
            }
            if (speed > 0)
            {
                speed -= STEPSPEED / 4f;
                if (speed < 0)
                    speed = 0;
            }
            else if (speed < 0)
            {
                speed += STEPSPEED / 4f;
                if (speed > 0)
                    speed = 0;
            }

            // motor
            joint[0].SetParam(DJointParameters.Velocity2, -speed);
            joint[0].SetParam(DJointParameters.FMax2, 0.1f);

            // steering
            float v = steer - joint[0].Angle1;
            if (v > 0.1) v = 0.1f;
            if (v < -0.1) v = -0.1f;
            v *= 10.0f;
            joint[0].SetParam(DJointParameters.Velocity, v);
            joint[0].SetParam(DJointParameters.FMax, 0.2f);
            joint[0].SetParam(DJointParameters.LoStop, -0.75f);
            joint[0].SetParam(DJointParameters.HiStop, 0.75f);
            joint[0].SetParam(DJointParameters.FudgeFactor, 0.1f);

            space.NearCallback = callback;
            space.Collide(null);
            ode.World.Step(0.05f);

            // remove all contact joints
            ode.Group.Empty();

            chassis.UpdateDisplay();
            wheels[0].UpdateDisplay();
            wheels[1].UpdateDisplay();
            wheels[2].UpdateDisplay();
        }

        public override void Initialize()
        {
            Framework.Camera.Position = new Vector3(2, 2, 3);
            Framework.Camera.Yaw = MathHelper.Pi / 180 * 30;
            Framework.Camera.Pitch = -MathHelper.Pi / 180 * 20;

            // create world
            ode = new Ode();
            ode.World.Gravity = new DVector3(0, -0.5f, 0);

            space = new DHashSpace(null);
            ground = new Entity<DPlane>(ode.World, new DPlane(space, 0, 1, 0, 0));

            ground.Display = new DisplayBox(Framework.GraphicsDevice, Color.LightGray, Matrix.CreateTranslation(0, -0.05f, 0), new Vector3(100, 0.1f, 100));
            Framework.Add(ground.Display);

            // chassis body
            chassis = new MovableEntity<DBox>(ode.World, new DBox(null, LENGTH, HEIGHT, WIDTH));
            chassis.Body.Position = new DVector3(0, STARTY, 0);
            DMass m = new DMass();
            m.SetBox(1, LENGTH, HEIGHT, WIDTH);
            m.Adjust(CMASS);
            chassis.Body.Mass = m;
            chassis.Display = new DisplayBox(Framework.GraphicsDevice, Color.Blue, chassis.Geometry.Lengths.ToXna());
            Framework.Add(chassis.Display);

            // wheel bodies
            DQuaternion q = DQuaternion.FromAxisAndAngle(1, 0, 0, MathHelper.PiOver2);
            m.SetSphere(1, RADIUS);
            m.Adjust(WMASS);
            wheels = new MovableEntity<DSphere>[3];
            for (int i = 0; i < 3; i++)
            {
                wheels[i] = new MovableEntity<DSphere>(ode.World, new DSphere(null, RADIUS));
                wheels[i].Body.Quaternion = q;
                wheels[i].Body.Mass = m;
            }
            wheels[0].Body.Position = new DVector3(0.5f * LENGTH, STARTY - HEIGHT * 0.5f, 0);
            wheels[1].Body.Position = new DVector3(-0.5f * LENGTH, STARTY - HEIGHT * 0.5f, WIDTH * 0.5f);
            wheels[2].Body.Position = new DVector3(-0.5f * LENGTH, STARTY - HEIGHT * 0.5f, -WIDTH * 0.5f);

            for (int i = 0; i < 3; i++)
            {
                wheels[i].Display = new DisplayCylinder(Framework.GraphicsDevice, Color.Green, wheels[i].Geometry.Radius, 0.02f);
                Framework.Add(wheels[i].Display);
            }

            // front and back wheel hinges
            for (int i = 0; i < 3; i++)
            {
                joint[i] = new DHinge2(ode.World, null);
                joint[i].Attach(chassis.Body, wheels[i].Body);
                DVector3 a = wheels[i].Body.Position;
                joint[i].Anchor1 = a;
                joint[i].Axis1 = new DVector3(0, 1, 0);
                joint[i].Axis2 = new DVector3(0, 0, 1);
            }

            // set joint suspension
            for (int i = 0; i < 3; i++)
            {
                joint[i].SetParam(DJointParameters.SuspensionERP, 0.4f);
                joint[i].SetParam(DJointParameters.SuspensionCFM, 0.8f);
            }

            // lock back wheels along the steering axis
            for (int i = 1; i < 3; i++)
            {
                // set stops to make sure wheels always stay in alignment
                joint[i].SetParam(DJointParameters.LoStop, 0f);
                joint[i].SetParam(DJointParameters.HiStop, 0f);
            }

            // create car space and add it to the top level space
            car_space = new DSimpleSpace(space);
            car_space.CleanupMode = 0;
            car_space.Add(chassis.Geometry);
            car_space.Add(wheels[0].Geometry);
            car_space.Add(wheels[1].Geometry);
            car_space.Add(wheels[2].Geometry);

            // environment
            ground_box = new Entity<DBox>(ode.World, new DBox(space, 2, 1, 1.5f));
            DMatrix3 R = DMatrix3.FromAxisAndAngle(0, 0, 1, 0.15f);
            ground_box.Geometry.Position = new DVector3(2, -0.34f, 0);
            ground_box.Geometry.Rotation = R;

            ground_box.Display = new DisplayBox(Framework.GraphicsDevice, Color.Gray, ground_box.Geometry.WorldMatrix.ToXna(),
                ground_box.Geometry.Lengths.ToXna());
            Framework.Add(ground_box.Display);
        }

        public override void Quit()
        {
            chassis.Dispose();
            wheels[0].Dispose();
            wheels[1].Dispose();
            wheels[2].Dispose();
            this.car_space.Dispose();
            this.ground.Dispose();
            this.ground_box.Dispose();
            this.joint[0].Dispose();
            this.joint[1].Dispose();
            this.joint[2].Dispose();
            space.Dispose();
            ode.Dispose();
        }

        public override string[] GetHelpString()
        {
            return new string[]{
                "Press: Up to increase speed.",
                "            Down to decrease speed.",
                "            Left to steer left.",
                "            Right to steer right.",
                "            Space to reset speed and steering."
            };
        }
        #endregion
    }
}
